/*****************************************************************
**          大连理工大学 凌BUG战队&大连民族大学 C.ONE战队
**                **生死有命，富贵在天**
**    没有什么难题是一个通宵解决不了的，如果有那就整两个！
**---------------------------------------------------------------
** 项目名称：   dlmu-hero-robot
** 日    期：   2024-03-12
** 作    者：   MasterWang & Changed by TuxMonkey
**---------------------------------------------------------------
** 文 件 名：   PID.c
** 文件说明：   PID控制算法
*****************************************************************/

/*---------------------INCLUDES----------------------*///在目前的调整中要注意调整时间和超调量的关系，也就是对于系统调节的“快”和“稳”
#include "PID.h"

/*---------------------VARIABLES---------------------*/  //kp:可以提高调节速度提高系统调节精度。当系统一旦出现了偏差，比例调节立即产生调节作用以减少误差，kp越大系统出现误差反应越快，但也容易产生过度调节
//使用CCM数据RAM区(0x10002000-0x10002FFF 4KB)            //kp的过大过度调节会使系统不断校正，从而导致系统在一定范围振荡
//
PID pid_chassis[4] __attribute__((at(0x10002000))); //底盘电机控速PID(4个M3508)
                                                     //ki：作用在于消除系统的稳态误差，提高无差度。 越大积分速度越快，系统静差消除越快，但过大在响应过程初期会产生积分饱和现象
																										 //过小会使积分作用变弱，使系统的静差难以消除，过渡时间加长，不能尽快达到稳定状态，影响系统的调节精度和动态特性
PID pid_gimbal[2] __attribute__((at(0x10002200))); //云台电机控速PID(2个GM6020)//此时系统超调，使动态性能变差   
PID pid_ammobooster[3] __attribute__((at(0x10002400))); //发射机构电机控速PID(2个M3508,1个M2006)  //影响系统的调节精度和动态特性  ki 过小难以消除每次运动的细微的误差
                                                                                
PID pid_yaw __attribute__((at(0x10002600))); //云台yaw轴姿态控位PID  
PID pid_pitch __attribute__((at(0x10002700))); //云台pitch轴姿态控位PID
PID pid_rotation __attribute__((at(0x10002800))); //底盘跟随云台控位PID

PID pid_gyrotemp __attribute__((at(0x10002900))); //BMI088控温PID

/*---------------------FUNCTIONS---------------------*/

/***********************************************************************kd:影响了系统的稳定程度，可以反映系统偏差信号的变化率并预见偏差变化的趋势，能产生超前的控制作用，使系统的超调降低，增加系统稳定性
** 函 数 名： InitPID()                                                    但 kd不能过大，过大则会使响应过程提前制动和延长系统调节时间，而且还会使系统的抗干扰性变差。也就是对于外界的干扰抵抗能力变差如步兵运动时的意外撞击等
** 函数说明： 初始化PID参数                                             
**---------------------------------------------------------------------
** 输入参数： 无
** 返回参数： 无
***********************************************************************/
void InitPID(void)
{
	uint8_t i;
	
	//底盘电机控速PID changed
	for(i=0;i<4;i++)
	{
		pid_chassis[i].Ref = 0.0f;
		pid_chassis[i].FeedBack = 0.0f;
		pid_chassis[i].Error = 0.0f;
		pid_chassis[i].DError = 0.0f;
		pid_chassis[i].DDError = 0.0f;
		pid_chassis[i].PreError = 0.0f;
		pid_chassis[i].PreDError = 0.0f;
		pid_chassis[i].IntegralError = 0.0f;
		pid_chassis[i].Kp = 3.0f;
		pid_chassis[i].Ki = 0.5f;
		pid_chassis[i].Kd = 0.3f;
		pid_chassis[i].MaxOutValue = 10000.0f;
		pid_chassis[i].MinOutValue = -10000.0f;
		pid_chassis[i].Out = 0.0f;
	}
	
	//云台电机控速PID(yaw,pitch)
	for(i=0;i<1;i++)
	{
		pid_gimbal[i].Ref = 0.0f;
		pid_gimbal[i].FeedBack = 0.0f;
		pid_gimbal[i].Error = 0.0f;
		pid_gimbal[i].DError = 0.0f;
		pid_gimbal[i].DDError = 0.0f;
		pid_gimbal[i].PreError = 0.0f;
		pid_gimbal[i].PreDError = 0.0f;
		pid_gimbal[i].IntegralError = 0.0f;
		pid_gimbal[i].Kp = 175.0f;
		pid_gimbal[i].Ki = 20.0f;
		pid_gimbal[i].Kd = 0.0f;
		pid_gimbal[i].MaxOutValue = 30000.0f;
		pid_gimbal[i].MinOutValue = -30000.0f;
		pid_gimbal[i].Out = 0.0f;
	}
		for(i=1;i<2;i++)
	{
		pid_gimbal[i].Ref = 0.0f;
		pid_gimbal[i].FeedBack = 0.0f;
		pid_gimbal[i].Error = 0.0f;
		pid_gimbal[i].DError = 0.0f;
		pid_gimbal[i].DDError = 0.0f;
		pid_gimbal[i].PreError = 0.0f;
		pid_gimbal[i].PreDError = 0.0f;
		pid_gimbal[i].IntegralError = 0.0f;
		pid_gimbal[i].Kp = 175.0f;
		pid_gimbal[i].Ki = 30.0f;
		pid_gimbal[i].Kd = 0.0f;
		pid_gimbal[i].MaxOutValue = 30000.0f;
		pid_gimbal[i].MinOutValue = -30000.0f;
		pid_gimbal[i].Out = 0.0f;
	}
	
	//摩擦轮电机控速PID
	for(i=0;i<2;i++)
	{
		pid_ammobooster[i].Ref = 0.0f;
		pid_ammobooster[i].FeedBack = 0.0f;
		pid_ammobooster[i].Error = 0.0f;
		pid_ammobooster[i].DError = 0.0f;
		pid_ammobooster[i].DDError = 0.0f;
		pid_ammobooster[i].PreError = 0.0f;
		pid_ammobooster[i].PreDError = 0.0f;
		pid_ammobooster[i].IntegralError = 0.0f;
		pid_ammobooster[i].Kp = 30.0f;//30
		pid_ammobooster[i].Ki = 20.0f;//20
		pid_ammobooster[i].Kd = 0.0f;
		pid_ammobooster[i].MaxOutValue = 16384.0f;//16384
		pid_ammobooster[i].MinOutValue = -16384.0f;
		pid_ammobooster[i].Out = 0.0f;
	}
	
	//拨弹电机控速PID
	pid_ammobooster[2].Ref = 0.0f;
	pid_ammobooster[2].FeedBack = 0.0f;
	pid_ammobooster[2].Error = 0.0f;
	pid_ammobooster[2].DError = 0.0f;
	pid_ammobooster[2].DDError = 0.0f;
	pid_ammobooster[2].PreError = 0.0f;
	pid_ammobooster[2].PreDError = 0.0f;
	pid_ammobooster[2].IntegralError = 0.0f;
	pid_ammobooster[2].Kp = 5.0f;//20
	pid_ammobooster[2].Ki = 5.0f;//10
	pid_ammobooster[2].Kd = 0.1f;
	pid_ammobooster[2].MaxOutValue = 16384.0f;
	pid_ammobooster[2].MinOutValue = -16384.0f;
	pid_ammobooster[2].Out = 0.0f;
	
	//云台姿态控位PID(yaw,pitch)
	pid_yaw.Ref = 0.0f;
	pid_yaw.FeedBack = 0.0f;
	pid_yaw.Error = 0.0f;
	pid_yaw.DError = 0.0f;
	pid_yaw.DDError = 0.0f;
	pid_yaw.PreError = 0.0f;
	pid_yaw.PreDError = 0.0f;
	pid_yaw.IntegralError = 0.0f;
	pid_yaw.Kp = 20.0f;
	pid_yaw.Ki = 0.0f;
	pid_yaw.Kd = 0.0f;
	pid_yaw.MaxOutValue = 1000.0f;
	pid_yaw.MinOutValue = -1000.0f;
	pid_yaw.Out = 0.0f;
	
	pid_pitch.Ref = 0.0f;
	pid_pitch.FeedBack = 0.0f;
	pid_pitch.Error = 0.0f;
	pid_pitch.DError = 0.0f;
	pid_pitch.DDError = 0.0f;
	pid_pitch.PreError = 0.0f;
	pid_pitch.PreDError = 0.0f;
	pid_pitch.IntegralError = 0.0f;
	pid_pitch.Kp = 40.0f;
	pid_pitch.Ki = 0.0f;
	pid_pitch.Kd = 0.0f;
	pid_pitch.MaxOutValue = 1000.0f;
	pid_pitch.MinOutValue = -1000.0f;
	pid_pitch.Out = 0.0f;
	
	//底盘跟随云台控位PID
	pid_rotation.Ref = 0.0f;
	pid_rotation.FeedBack = 0.0f;
	pid_rotation.Error = 0.0f;
	pid_rotation.DError = 0.0f;
	pid_rotation.DDError = 0.0f;
	pid_rotation.PreError = 0.0f;
	pid_rotation.PreDError = 0.0f;
	pid_rotation.IntegralError = 0.0f;
	pid_rotation.Kp = 1.0f;
	pid_rotation.Ki = 0.0f;
	pid_rotation.Kd = 0.0f;
	pid_rotation.MaxOutValue = 45.0f;
	pid_rotation.MinOutValue = -45.0f;
	pid_rotation.Out = 0.0f;
	
	//BMI088控温PID
	pid_gyrotemp.Ref = 45.0f;
	pid_gyrotemp.FeedBack = 0.0f;
	pid_gyrotemp.Error = 0.0f;
	pid_gyrotemp.DError = 0.0f;
	pid_gyrotemp.DDError = 0.0f;
	pid_gyrotemp.PreError = 0.0f;
	pid_gyrotemp.PreDError = 0.0f;
	pid_gyrotemp.IntegralError = 0.0f;
	pid_gyrotemp.Kp = 10.0f;
	pid_gyrotemp.Ki = 5.0f;
	pid_gyrotemp.Kd = 0.0f;
	pid_gyrotemp.MaxOutValue = 1000.0f; //2000.0f
	pid_gyrotemp.MinOutValue = 0.0f;
	pid_gyrotemp.Out = 300.0f;
}

/***********************************************************************
** 函 数 名： PIDSetPara()
** 函数说明： 设置PID参数
**---------------------------------------------------------------------
** 输入参数： PID结构体指针,kp系数,ki系数,kd系数
** 返回参数： 无
***********************************************************************/
void PIDSetPara(PID *pid, float kp, float ki, float kd)
{
	pid->Kp = kp;
	pid->Ki = ki;
	pid->Kd = kd;
}

/***********************************************************************
** 函 数 名： PIDIncControl()
** 函数说明： 增量式PID控制计算
**---------------------------------------------------------------------
** 输入参数： PID结构体指针,反馈值
** 返回参数： 输出值
***********************************************************************/
float PIDIncControl(PID *pid, float fdb)
{
	//
	pid->FeedBack = fdb;

	//
	pid->Error = pid->Ref - pid->FeedBack;
	pid->DError = pid->Error - pid->PreError;
	pid->DDError = pid->DError - pid->PreDError;
	
	pid->PreError = pid->Error;
	pid->PreDError = pid->DError;

	//
	pid->Out += (pid->Kp * pid->DError + pid->Ki * pid->Error / 10 + pid->Kd * pid->DDError);

	//
	if(pid->Out > pid->MaxOutValue)
	{
		 pid->Out = pid->MaxOutValue;	
	}
	if(pid->Out < pid->MinOutValue)
	{
		 pid->Out = pid->MinOutValue;
	}

	//
	return pid->Out;
}

/***********************************************************************
** 函 数 名： PIDPosControl()
** 函数说明： 位置式PID控制计算
**---------------------------------------------------------------------
** 输入参数： PID结构体指针,反馈值
** 返回参数： 输出值
***********************************************************************/
float PIDPosControl(PID *pid, float fdb)
{
	//
	pid->FeedBack = fdb;

	//
	pid->Error = pid->Ref - pid->FeedBack;
	pid->IntegralError	+=	pid->Error;
	pid->DError = pid->Error - pid->PreError;
	
	pid->PreError = pid->Error;

	//
	pid->Out = (pid->Kp * pid->Error + pid->Ki * pid->IntegralError / 10 + pid->Kd * pid->DError * 100);

	//
	if(pid->Out > pid->MaxOutValue)
	{
		 pid->Out = pid->MaxOutValue;	
	}
	if(pid->Out < pid->MinOutValue)
	{
		 pid->Out = pid->MinOutValue;
	}

	//
	return pid->Out;
}
